//
// Created by 马皓然 on 2025/10/25.
//

#ifndef XIAOMI_TEST_MOTION_PLANNER_H
#define XIAOMI_TEST_MOTION_PLANNER_H
#include "stm32f4xx_hal.h"
#include <stdbool.h>
#include "motor_manager.h" // 依赖电机管理层，用于更新目标值

// 梯形规划器数据结构
typedef struct {
    float start_pos;      // 运动起始位置 (通常为电机当前位置)
    float target_pos;     // 运动目标位置 (用户设定)
    float max_vel;        // 规划器允许的最大速度 (V_max)
    float accel;          // 规划器允许的最大加速度/减速度 (A_max)

    float current_time;   // 当前运行时间 (ms 或 s)
    float total_time;     // 总运行时间

    // 轨迹输出
    float pos_output;     // 当前周期的目标位置
    float vel_output;     // 当前周期的目标速度

    bool is_planning;     // 运动是否正在进行
} Trapezoid_Planner_t;

// 函数声明
void Planner_Init(Trapezoid_Planner_t *planner);

void Planner_Start_Motion(Trapezoid_Planner_t *planner,
                          float target_pos,
                          float max_vel,
                          float accel);

void Planner_Update(Trapezoid_Planner_t *planner, float cycle_time_s);

void Planner_Apply_To_Motor(const Trapezoid_Planner_t *planner, Motor_Instance_t *motor);
#endif //XIAOMI_TEST_MOTION_PLANNER_H